#include <iostream>
#include <random>
#include <dds/dds.h>
#include "start_atomic_0826_data_access_service.h" // IDL生成的C语言头文件
#include "../../../include/log_client/LogClient.h"

// DDS实体句柄
static dds_entity_t participant;
static dds_entity_t topic;
static dds_entity_t publisher;
static dds_entity_t writer;

// 清理DDS资源
static void cleanup()
{
    if (writer != DDS_HANDLE_NIL)
        dds_delete(writer);
    if (publisher != DDS_HANDLE_NIL)
        dds_delete(publisher);
    if (topic != DDS_HANDLE_NIL)
        dds_delete(topic);
    if (participant != DDS_HANDLE_NIL)
        dds_delete(participant);
}

// 生成随机导航数据
start_atomic_0826_data_access_service_NavDataInput generate_random_nav_data_input()
{
    // 静态随机数生成器（确保全局只初始化一次，提升性能）
    static std::mt19937 rng(std::random_device{}());

    // 定义各成员的合理取值范围分布
    std::uniform_real_distribution<double> gps_time_dist(1620000000.0, 1630000000.0); // GPS时间（示例：2021年前后的时间戳）
    std::uniform_real_distribution<double> gps_lat_dist(-90.0, 90.0);                // 纬度（-90° ~ 90°）
    std::uniform_real_distribution<double> gps_lon_dist(-180.0, 180.0);               // 经度（-180° ~ 180°）
    std::uniform_real_distribution<float> gps_alt_dist(-400.0, 8848.0);               // 高度（-400m~珠峰高度）
    std::uniform_real_distribution<float> gps_hdop_dist(0.5f, 20.0f);                 // 水平精度因子（0.5~20，越小越精确）
    
    std::uniform_real_distribution<double> imu_time_dist(1620000000.0, 1630000000.0); // IMU时间戳
    std::uniform_real_distribution<float> imu_acc_dist(-20.0f, 20.0f);                // 加速度（-20~20 m/s²）
    std::uniform_real_distribution<float> imu_gyro_dist(-10.0f, 10.0f);               // 角速度（-10~10 rad/s）
    
    std::uniform_real_distribution<double> odom_time_dist(1620000000.0, 1630000000.0); // 里程计时间戳
    std::uniform_real_distribution<float> odom_speed_dist(0.0f, 30.0f);                // 速度（0~30 m/s）
    std::uniform_real_distribution<float> odom_heading_dist(-180.0f, 180.0f);          // 航向角（-180°~180°）

    // 初始化结构体并填充随机值
    start_atomic_0826_data_access_service_NavDataInput data;
    data.gps_time = gps_time_dist(rng);
    data.gps_latitude = gps_lat_dist(rng);
    data.gps_longitude = gps_lon_dist(rng);
    data.gps_altitude = gps_alt_dist(rng);
    data.gps_hdop = gps_hdop_dist(rng);
    
    data.imu_time = imu_time_dist(rng);
    data.imu_acc_x = imu_acc_dist(rng);
    data.imu_acc_y = imu_acc_dist(rng);
    data.imu_acc_z = imu_acc_dist(rng);
    data.imu_gyro_x = imu_gyro_dist(rng);
    data.imu_gyro_y = imu_gyro_dist(rng);
    data.imu_gyro_z = imu_gyro_dist(rng);
    
    data.odom_time = odom_time_dist(rng);
    data.odom_speed = odom_speed_dist(rng);
    data.odom_heading = odom_heading_dist(rng);

    return data;
}

int main()
{
    // 初始化日志
    LogClient::getInstance().init("mock_atomic_0826_data_access_service","mock_atomic_0826_data_access_service");

    dds_return_t rc;

    // 1. 初始化DDS参与者（DomainParticipant）
    participant = dds_create_participant(DDS_DOMAIN_DEFAULT, NULL, NULL);
    if (participant < 0)
    {
        std::cerr << "错误：创建参与者失败，原因：" << dds_strretcode(-participant) << std::endl;
        LogClient::getInstance().logError("创建参与者失败，原因：" + std::string(dds_strretcode(-participant)));
        cleanup();
        return EXIT_FAILURE;
    }

    // 2. 创建主题（Topic），名称需与订阅者一致
    topic = dds_create_topic(
        participant,
        &start_atomic_0826_data_access_service_NavDataInput_desc, // IDL生成的类型描述
        "start_atomic_0826_data_access_service",                      // 主题名称（必须与订阅者一致）
        NULL, NULL);
    if (topic < 0)
    {
        std::cerr << "错误：创建主题失败，原因：" << dds_strretcode(-topic) << std::endl;
        LogClient::getInstance().logError("创建主题失败，原因：" + std::string(dds_strretcode(-topic)));
        cleanup();
        return EXIT_FAILURE;
    }

    // 3. 创建发布者（Publisher）
    publisher = dds_create_publisher(participant, NULL, NULL);
    if (publisher < 0)
    {
        std::cerr << "错误：创建发布者失败，原因：" << dds_strretcode(-publisher) << std::endl;
        LogClient::getInstance().logError("创建发布者失败，原因：" + std::string(dds_strretcode(-publisher)));
        cleanup();
        return EXIT_FAILURE;
    }

    dds_qos_t *qos = dds_create_qos();
    dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_SECS(10));
    dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 10);

    // 4. 创建数据写入器（DataWriter）
    writer = dds_create_writer(publisher, topic, qos, NULL);
    if (writer < 0)
    {
        std::cerr << "错误：创建写入器失败，原因：" << dds_strretcode(-writer) << std::endl;
        LogClient::getInstance().logError("创建写入器失败，原因：" + std::string(dds_strretcode(-writer)));
        cleanup();
        return EXIT_FAILURE;
    }
    dds_delete_qos(qos);

    std::cout << "DDS导航数据发布者已启动，开始发送数据..." << std::endl;
    LogClient::getInstance().logInfo("DDS导航数据发布者已启动，开始发送数据...");

    // 5. 循环发送数据（持续发送，每次间隔1秒）
    int i = 0;
    while (1)
    {
        // 生成随机导航数据
        start_atomic_0826_data_access_service_NavDataInput data = generate_random_nav_data_input();

        // 发送数据
        rc = dds_write(writer, &data);
        if (rc != DDS_RETCODE_OK)
        {
            std::cerr << "错误：dds_write失败，原因：" << dds_strretcode(-rc) << std::endl;
            LogClient::getInstance().logError("dds_write失败，原因：" + std::string(dds_strretcode(-rc)));
            cleanup();
            return EXIT_FAILURE;
        }

        // 打印发送的信息
        std::string info = "已发送导航数据（第" + std::to_string(i + 1) + "次）：\n";
        info += "  GPS时间：" + std::to_string(data.gps_time) + "\n";
        info += "  GPS纬度：" + std::to_string(data.gps_latitude) + "°\n";
        info += "  GPS经度：" + std::to_string(data.gps_longitude) + "°\n";
        info += "  GPS高度：" + std::to_string(data.gps_altitude) + "m\n";
        info += "  GPS水平精度因子：" + std::to_string(data.gps_hdop) + "\n";
        info += "  IMU时间：" + std::to_string(data.imu_time) + "\n";
        info += "  IMU加速度(x,y,z)：" + std::to_string(data.imu_acc_x) + "," 
                + std::to_string(data.imu_acc_y) + "," + std::to_string(data.imu_acc_z) + " m/s²\n";
        info += "  IMU角速度(x,y,z)：" + std::to_string(data.imu_gyro_x) + "," 
                + std::to_string(data.imu_gyro_y) + "," + std::to_string(data.imu_gyro_z) + " rad/s\n";
        info += "  里程计时间：" + std::to_string(data.odom_time) + "\n";
        info += "  里程计速度：" + std::to_string(data.odom_speed) + " m/s\n";
        info += "  里程计航向角：" + std::to_string(data.odom_heading) + "°";

        std::cout << "==============================" << std::endl;
        std::cout << info << std::endl;
        std::cout << "==============================" << std::endl;
        LogClient::getInstance().logInfo(info);

        // 间隔1秒发送
        i++;
        dds_sleepfor(DDS_SECS(1));
    }

    // 清理资源
    cleanup();
    return EXIT_SUCCESS;
}